{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 260,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import time\n",
    "#import math\n",
    "from sympy.geometry import Circle, Point\n",
    "# import rospy\n",
    "# from nav_msgs.msg import OccupancyGrid\n",
    "# from std_msgs.msg import Float32\n",
    "import matplotlib.pyplot as plt\n",
    "# Vehicle Parameters\n",
    "\n",
    "# from visualization_msgs.msg import Marker\n",
    "# from visualization_msgs.msg import MarkerArray\n",
    "# from geometry_msgs.msg import Quaternion, Pose, Point, Vector3\n",
    "# from std_msgs.msg import Header, ColorRGBA\n",
    "\n",
    "SPEED = 1.4\n",
    "LENGTH = 1.5\n",
    "WHEEL_BASE = 1.0\n",
    "STEERING_RATIO = 1\n",
    "vehicle_points = np.array([[-1.1, 2],[0, 2],[1.1, 2],[-1.1, 0],[0, 0],[1.1,0],[-1.1, -2],[0, -2],[1.1, -2]])\n",
    "\n",
    "\n",
    "max_steering_rate = 0.0872665\n",
    "left_limit_1 = 2.35619\n",
    "left_limit_2 = 3.92699\n",
    "right_limit_1 = 0.785398\n",
    "right_limit_2 = -0.785398\n",
    "max_angle = 0.785398\n",
    "min_angle = -0.785398\n",
    "\n",
    "\n",
    "### State Model\n",
    "\n",
    "class state:\n",
    "\n",
    "    def __init__(self, x, y, theta, g,f,h, steer):\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.theta = theta\n",
    "        self.g = g\n",
    "        self.f = f\n",
    "        self.h = h\n",
    "        self.steer = steer\n",
    "\n",
    "\n",
    "### State Transition\n",
    "\n",
    "def transition(v_state, delta, dt, goal):\n",
    "   \n",
    "    # ---Begin Vehicle model---\n",
    "    \n",
    "    omega = SPEED/LENGTH * np.tan(delta)\n",
    "    \n",
    "    \n",
    "    d_x = SPEED * np.cos(v_state.theta) * dt\n",
    "    d_y = SPEED * np.sin(v_state.theta) * dt\n",
    "    \n",
    "    next_x = v_state.x + d_x\n",
    "    next_y = v_state.y + d_y\n",
    "    next_theta = normalize(v_state.theta + (omega*dt)) \n",
    "\n",
    "    # ---End Vehicle model---\n",
    "    next_g = v_state.g + np.sqrt( d_x * d_x +  d_y * d_y )\n",
    "    next_h = heuristic_2(next_x, next_y, goal)\n",
    "    \n",
    "    next_f = next_g + next_h + 0.1*(delta)  \n",
    "    next_steer = delta\n",
    "\n",
    "    # Create a new State object with all of the \"next\" values.\n",
    "    new_state = state(next_x, next_y, next_theta, next_g, next_f, next_h, next_steer)\n",
    "\n",
    "    return new_state\n",
    "\n",
    "\n",
    "### Grid Resolution\n",
    "\n",
    "grid_x_m = 20\n",
    "grid_y_m = 20\n",
    "\n",
    "coll_cell_side = 1.0\n",
    "grid_coll_x = np.int( np.ceil(grid_x_m/coll_cell_side) )\n",
    "grid_coll_y = np.int( np.ceil(grid_y_m/coll_cell_side) )\n",
    "\n",
    "grid = np.zeros((grid_coll_x,grid_coll_y), np.int)\n",
    "\n",
    "cell_side = 1.0\n",
    "x_offset = (grid.shape[1]*coll_cell_side)/2\n",
    "y_offset = (grid.shape[0]*coll_cell_side)/2\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "### Grid with Obstacles\n",
    "\n",
    "\n",
    "### A Class to store  every  state and the index\n",
    "\n",
    "\n",
    "class link:\n",
    "    def __init__(self, inp_state, inp_index):\n",
    "        self.state = inp_state\n",
    "        self.index = inp_index\n",
    "        \n",
    "\n",
    "\n",
    "### Start And Goal\n",
    "\n",
    "class start:\n",
    "\n",
    "    def __init__(self, x, y, theta):\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.theta = theta\n",
    "        \n",
    "class goal:\n",
    "\n",
    "    def __init__(self, x, y):\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "\n",
    "\n",
    "### Helper Functions\n",
    "def angle_between_two_points(x,y, goal):\n",
    "    myradians = math.atan2(goal.y-y, goal.x-x)\n",
    "    #print(myradians)\n",
    "    return myradians\n",
    "\n",
    "def lengthSquare(point1, point2):\n",
    "    return (point1[0]-point2[0])**2 + (point1[1]-point2[1])**2\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "    \n",
    "def get_angle(A, B, C):\n",
    "    # Square of lengths be a2, b2, c2\n",
    "    a2 = lengthSquare(B,C)\n",
    "    b2 = lengthSquare(A,C)\n",
    "    c2 = lengthSquare(A,B)\n",
    "\n",
    "    # lenght of sides be a, b, c\n",
    "    a = np.sqrt(a2)\n",
    "    b = np.sqrt(b2)\n",
    "    c = np.sqrt(c2)\n",
    "\n",
    "    # From Cosine law\n",
    "    alpha = np.cos((b2 + c2 - a2)/(2*b*c))\n",
    "    beta = np.cos((a2 + c2 - b2)/(2*a*c))\n",
    "    gamma = np.cos((a2 + b2 - c2)/(2*a*b))\n",
    "\n",
    "        #// Converting to degree\n",
    "#     alpha = alpha * 180 / np.pi\n",
    "#     betta = betta * 180 / np.pi\n",
    "#     gamma = gamma * 180 / np.pi\n",
    "\n",
    "    return alpha, beta, gamma\n",
    "\n",
    "def get_angle_special(A, B, C):\n",
    "    # Square of lengths be a2, b2, c2\n",
    "    a2 = lengthSquare(B,C)\n",
    "    b2 = lengthSquare(A,C)\n",
    "    c2 = lengthSquare(A,B)\n",
    "\n",
    "    # lenght of sides be a, b, c\n",
    "    a = np.sqrt(a2)\n",
    "    b = np.sqrt(b2)\n",
    "    c = np.sqrt(c2)\n",
    "\n",
    "    # From Cosine law\n",
    "    alpha = 6.28319 - np.cos((b2 + c2 - a2)/(2*b*c))\n",
    "    beta = 6.28319 - np.cos((a2 + c2 - b2)/(2*a*c))\n",
    "    gamma = 6.28319 - np.cos((a2 + b2 - c2)/(2*a*b))\n",
    "\n",
    "        #// Converting to degree\n",
    "#     alpha = alpha * 180 / np.pi\n",
    "#     betta = betta * 180 / np.pi\n",
    "#     gamma = gamma * 180 / np.pi\n",
    "\n",
    "    return alpha, beta, gamma\n",
    "\n",
    "def arclength(radius, beta):\n",
    "    arc_length = radius * beta\n",
    "    return arc_length\n",
    "    \n",
    "\n",
    "def deg_to_rad(delta):\n",
    "    return delta * math.pi/180\n",
    "\n",
    "def heuristic(x,y,goal):\n",
    "    distance  = np.sqrt( ((x-goal.x)**2) + ((y-goal.y)**2) )\n",
    "    return distance\n",
    "\n",
    "def heuristic_2 (x,y,goal):\n",
    "    myradians = angle_between_two_points(x,y, goal)\n",
    "    print(myradians)\n",
    "    if (myradians < 0) | (myradians>2*np.pi):\n",
    "        myradians = normalize_rad(myradians)\n",
    "        \n",
    "    \n",
    "    print(myradians)\n",
    "    \n",
    "    if myradians == 1.5707963267948966:\n",
    "        total_distance = heuristic(x,y, goal)\n",
    "        return total_distance\n",
    "    \n",
    "    \n",
    "    elif ((deg_to_rad(91) < myradians< deg_to_rad(180))): \n",
    "        \n",
    "        alpha_c,beta_c = -3.0, 0.0\n",
    "        radius = 3.0\n",
    "        print('1')\n",
    "        target = [goal.x, goal.y]\n",
    "        x2,y2,radius2 = mid_circle(alpha_c,beta_c,goal)            \n",
    "        circle1 = [[alpha_c,beta_c],radius]\n",
    "        circle2 = [[x2,y2], radius2]\n",
    "        tangent_point = tang_from_point(circle1, circle2)\n",
    "       \n",
    "        alpha, beta, gamma = get_angle([x,y],[alpha_c,beta_c], tangent_point[0])\n",
    "        distance_on_arc = arclength(radius, beta)\n",
    "        distance_from_point = np.sqrt( ((target[0]-tangent_point[0][0])**2) + ((target[1]-tangent_point[0][1])**2) )\n",
    "        total_distance = distance_on_arc + distance_from_point\n",
    "        print(total_distance ,distance_from_point,  distance_on_arc)\n",
    "        return total_distance\n",
    "#print(heuristic_2 (x,y,delta,Goal))\n",
    "            #distance_from_center = np.sqrt( ((target[0]-alpha_c)**2) + ((target[1]-beta_c)**2) )\n",
    "            \n",
    "    elif (deg_to_rad(0) < myradians< deg_to_rad(89)):\n",
    "        alpha_c,beta_c = 3.0, 0.0\n",
    "        radius = 3.0\n",
    "        print('2')\n",
    "        target = [goal.x, goal.y]\n",
    "        x2,y2,radius2 = mid_circle(alpha_c,beta_c,goal)            \n",
    "        circle1 = [[alpha_c,beta_c],radius]\n",
    "        circle2 = [[x2,y2], radius2]\n",
    "        tangent_point = tang_from_point(circle1, circle2)\n",
    "        alpha, beta, gamma = get_angle([x,y],[alpha_c,beta_c], tangent_point[0])\n",
    "        distance_on_arc = arclength(radius, beta)\n",
    "        distance_from_point = np.sqrt( ((target[0]-tangent_point[0][0])**2) + ((target[1]-tangent_point[0][1])**2) )\n",
    "        total_distance = distance_on_arc + distance_from_point\n",
    "        print(total_distance ,distance_from_point,  distance_on_arc)\n",
    "        return total_distance\n",
    "    \n",
    "    elif (deg_to_rad(181) < myradians< deg_to_rad(270)):\n",
    "        alpha_c,beta_c = -3.0, 0.0\n",
    "        radius = 3.0\n",
    "        print('3')\n",
    "        target = [goal.x, goal.y]\n",
    "        x2,y2,radius2 = mid_circle(alpha_c,beta_c,goal)            \n",
    "        circle1 = [[alpha_c,beta_c],radius]\n",
    "        circle2 = [[x2,y2], radius2]\n",
    "        tangent_point = tang_from_point(circle1, circle2)\n",
    "        alpha, beta, gamma = get_angle_special([x,y],[alpha_c,beta_c], tangent_point[0])\n",
    "        distance_on_arc = arclength(radius, beta)\n",
    "        distance_from_point = np.sqrt( ((target[0]-tangent_point[0][0])**2) + ((target[1]-tangent_point[0][1])**2) )\n",
    "        total_distance = distance_on_arc + distance_from_point\n",
    "        print(total_distance ,distance_from_point,  distance_on_arc)\n",
    "        return total_distance\n",
    "    \n",
    "    elif (deg_to_rad(271) < myradians< deg_to_rad(359)):\n",
    "        alpha_c,beta_c = 3.0, 0.0\n",
    "        radius = 3.0\n",
    "        print('4')\n",
    "        target = [goal.x, goal.y]\n",
    "        x2,y2,radius2 = mid_circle(alpha_c,beta_c,goal)            \n",
    "        circle1 = [[alpha_c,beta_c],radius]\n",
    "        circle2 = [[x2,y2], radius2]\n",
    "        tangent_point = tang_from_point(circle1, circle2)\n",
    "        alpha, beta, gamma = get_angle_special([x,y],[alpha_c,beta_c], tangent_point[0])\n",
    "        distance_on_arc = arclength(radius, beta)\n",
    "        distance_from_point = np.sqrt( ((target[0]-tangent_point[0][0])**2) + ((target[1]-tangent_point[0][1])**2) )\n",
    "        total_distance = distance_on_arc + distance_from_point\n",
    "        print(total_distance ,distance_from_point,  distance_on_arc)\n",
    "        return total_distance\n",
    "    \n",
    "    \n",
    "\n",
    "            \n",
    "        \n",
    "    \n",
    "\n",
    "\n",
    "\n",
    "def normalize(theta):\n",
    "    if theta < 0:    \n",
    "        theta = theta + 2.0 * np.pi\n",
    "        return theta\n",
    "    if theta > 2*np.pi:    \n",
    "        theta = theta - 2.0 * np.pi\n",
    "        return theta\n",
    "    else:\n",
    "        return theta \n",
    "\n",
    "def normalize_rad(radians):\n",
    "    if radians < 0:    \n",
    "        radians = radians + 2.0 * np.pi\n",
    "        return radians\n",
    "    if radians > 2*np.pi:    \n",
    "        radians = radians - 2.0 * np.pi\n",
    "        return radians\n",
    "    else:\n",
    "        return radians \n",
    "\n",
    "\n",
    "\n",
    "def rotation(w,t):\n",
    "    y = np.array([[np.cos(t), np.sin(t)], [-np.sin(t), np.cos(t)]])\n",
    "    return np.matmul(w, y)\n",
    "    \n",
    "\n",
    "def transformation(w, x, y,t):\n",
    "    p = np.zeros_like(w)\n",
    "    for i in range(p.shape[0]):\n",
    "        p[i][0:2] = w[i][0:2]+[x,y]\n",
    "        \n",
    "    return rotation(p,(t-np.pi/2))\n",
    "    \n",
    "def mid_circle(x,y,goal):\n",
    "    x1 = x\n",
    "    y1 = y\n",
    "    radius = heuristic(x,y,goal)/2\n",
    "    x2 = goal.x\n",
    "    y2 = goal.y\n",
    "\n",
    "    x_m_point = (x1 + x2)/2\n",
    "    y_m_point = (y1 + y2)/2\n",
    "    return (x_m_point, y_m_point, radius)\n",
    "\n",
    "def tang_from_point(circle1, circle2): # circle = [[α, β], r] \n",
    "\n",
    "    x1,y1 = circle1[0][0], circle1[0][1]\n",
    "    r1  = circle1[1] \n",
    "    x2,y2 = circle2[0][0], circle2[0][1]\n",
    "    r2 = circle2[1]\n",
    "    c1=Circle(Point(x1,y1),r1)\n",
    "    c2=Circle(Point(x2,y2),r2)\n",
    "    intersection = c1.intersection(c2)\n",
    "    #print(len(intersection))\n",
    "    if len(intersection) == 1:\n",
    "        intersection.append(intersection[0])\n",
    "    #print(intersection)\n",
    "    p1 = intersection[0]\n",
    "    p2 = intersection[1]\n",
    "    xs1,ys1 = p1.x,p1.y\n",
    "    xs2,ys2 = p2.x,p2.y\n",
    "    return [(float(xs1),float(ys1)),(float(xs2),float(ys2))]\n",
    "\n",
    "\n",
    "#def get_grid_coordinates(x, y):\n",
    "    #x = x+x_offset\n",
    "    #y = y+y_offset\n",
    "    #cell_x = np.floor(x/cell_side)\n",
    "    #cell_y = np.floor(y/cell_side)\n",
    "    #grid[cell_y, cell_x] = 2\n",
    "    #return cell_x, cell_y\n",
    "    \n",
    "\n",
    "\n",
    "def check_occupancy(x, y):\n",
    "    x = x+x_offset\n",
    "    y = y+y_offset\n",
    "    cell_x = np.floor(x/coll_cell_side)\n",
    "    cell_y = np.floor(y/coll_cell_side)\n",
    "    #grid[cell_y, cell_x] = 2\n",
    "    return grid[int(cell_y), int(cell_x)]\n",
    "\n",
    "\n",
    "def delta_t(delta, n, off):\n",
    "\n",
    "    delta_list = []\n",
    "    delta_list.append(delta)\n",
    "    delta_calc = delta\n",
    "    for i in range(0 ,n):\n",
    "        delta_calc += off   \n",
    "        if delta_calc < max_angle:\n",
    "            delta_list.append(delta_calc)\n",
    "        \n",
    "    delta_calc = delta\n",
    "    for i in range(0 ,n):\n",
    "        delta_calc -= off\n",
    "        if min_angle < delta_calc:\n",
    "            delta_list.append(delta_calc)\n",
    "        \n",
    "    return delta_list\n",
    "\n",
    "\n",
    "def checkLimits( x,y ):\n",
    "    \n",
    "    result = True\n",
    "    \n",
    "    if  ( x < -grid_x_m/2 ) | ( y < -grid_y_m/2 ) | ( x > grid_x_m/2 ) | ( y > grid_y_m/2 ):\n",
    "        \n",
    "        result = False\n",
    "    \n",
    "    return result\n",
    "\n",
    "state_cell_side = 1.0\n",
    "grid_state_x = np.int( np.ceil(grid_x_m/state_cell_side) )\n",
    "grid_state_y = np.int( np.ceil(grid_y_m/state_cell_side) )\n",
    "\n",
    "NUM_THETA_CELLS = 360\n",
    "\n",
    "\n",
    "x_state_offset = grid_x_m/2.0\n",
    "y_state_offset = grid_y_m/2.0\n",
    "\n",
    "\n",
    "def check_closed(x, y, theta):\n",
    "    x = x + x_state_offset\n",
    "    y = y + y_state_offset\n",
    "    \n",
    "    cell_x = np.floor(x/state_cell_side)\n",
    "    cell_y = np.floor(y/state_cell_side)\n",
    "    \n",
    "    cell_theta = np.floor( theta * NUM_THETA_CELLS / (2.0 * np.pi) )\n",
    "    return closed[int(cell_theta)][int(cell_y)][int(cell_x)]\n",
    "    \n",
    "\n",
    "def set_closed(x, y, theta):\n",
    "    x = x + x_state_offset\n",
    "    y = y + y_state_offset\n",
    "    \n",
    "    cell_x = np.floor(x/state_cell_side)\n",
    "    cell_y = np.floor(y/state_cell_side)\n",
    "    \n",
    "    cell_theta = np.floor( theta * NUM_THETA_CELLS / (2.0 * np.pi) )\n",
    "   \n",
    "    closed[int(cell_theta)][int(cell_y)][int(cell_x)] = 1\n",
    "\n",
    "\n",
    "current_state = state(0.0, 0.0, np.pi/2, 0.0, 0.0, 0.0, 0.0)\n",
    "new_points = np.copy(vehicle_points)\n",
    "#print(new_points)\n",
    "Goal = goal( 0.0, 5.0 )\n",
    "\n",
    "\n",
    "closed = [[[0 for x in range(grid_state_x)] for y in range(grid_state_y)] for cell in range(NUM_THETA_CELLS)]\n",
    "\n",
    "path_state = current_state\n",
    "open_list = []\n",
    "path_link = link( path_state, 0 )\n",
    "open_list.append( path_link )\n",
    "\n",
    "trace_list = []\n",
    "trace_link = link( path_state, -1 )\n",
    "trace_list.append( trace_link )\n",
    "\n",
    "\n",
    "path_x = []\n",
    "path_y = []\n",
    "path_h = []\n",
    "\n",
    "path_x.append( current_state.x )\n",
    "path_y.append( current_state.y )\n",
    "\n",
    "\n",
    "  \n",
    "# def listener():\n",
    "\n",
    "\n",
    "\n",
    "#     rospy.Subscriber( \"obstacle_grid\", OccupancyGrid, callback, queue_size=1 )\n",
    "#     rospy.Subscriber( \"/currentSteerAngle\", Float32, steerCallback )\n",
    "\n",
    "\n",
    "#     rospy.spin()\n",
    "\n",
    "# def search_service():\n",
    "\n",
    "#     print( \"Steer : \", search() )  \n",
    "\n",
    "            \n",
    "    \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 249,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-0.8960553845713439\n",
      "-0.8960553845713439\n"
     ]
    }
   ],
   "source": [
    "x = 0\n",
    "y = 0\n",
    "Goal = goal(4,-5)\n",
    "def angle_between_two_points(x,y, goal):\n",
    "    myradians = math.atan2(goal.y-y, goal.x-x)\n",
    "    print(myradians)\n",
    "    return myradians\n",
    "print(angle_between_two_points(x,y, Goal))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 119,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "3.9269908169872414"
      ]
     },
     "execution_count": 119,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "deg_to_rad(225)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 239,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-2.356194490192345"
      ]
     },
     "execution_count": 239,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "x , y = 0,0\n",
    "t = goal(-15,-15)\n",
    "myradians = angle_between_two_points(x,y,t)\n",
    "myradians\n",
    "#print((deg_to_rad(91) < myradians< deg_to_rad(180)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 252,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = goal(0,15)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 218,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "21.213203435596427"
      ]
     },
     "execution_count": 218,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "heuristic(0,0, t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 255,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "15.0\n"
     ]
    }
   ],
   "source": [
    "x = 0\n",
    "y = 0\n",
    "Goal = goal(0,15)\n",
    "delta = 1\n",
    "\n",
    "print(heuristic_2 (x,y,Goal))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 204,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0\n",
      "[]\n"
     ]
    },
    {
     "ename": "IndexError",
     "evalue": "list index out of range",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mIndexError\u001b[0m                                Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-204-f1818a6270c0>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m     17\u001b[0m     \u001b[0mintersection\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mappend\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mintersection\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     18\u001b[0m \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mintersection\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 19\u001b[0;31m \u001b[0mp1\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mintersection\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     20\u001b[0m \u001b[0mp2\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mintersection\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     21\u001b[0m \u001b[0mxs1\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mys1\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mp1\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mx\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mp1\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0my\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mIndexError\u001b[0m: list index out of range"
     ]
    }
   ],
   "source": [
    "circle1 = [[3,0],3]\n",
    "x = 3\n",
    "y = 0\n",
    "Goal = goal(-5,-5)\n",
    "\n",
    "x2,y2,radius2 = mid_circle(x,y,Goal)     \n",
    "circle2 = [[x2,y2],radius2]\n",
    "x1,y1 = circle1[0][0], circle1[0][1]\n",
    "r1  = circle1[1] \n",
    "x2,y2 = circle2[0][0], circle2[0][1]\n",
    "r2 = circle2[1]\n",
    "c1=Circle(Point(x1,y1),r1)\n",
    "c2=Circle(Point(x2,y2),r2)\n",
    "intersection = c1.intersection(c2)\n",
    "print(len(intersection))\n",
    "if len(intersection) == 1:\n",
    "    intersection.append(intersection[0])\n",
    "print(intersection)\n",
    "p1 = intersection[0]\n",
    "p2 = intersection[1]\n",
    "xs1,ys1 = p1.x,p1.y\n",
    "xs2,ys2 = p2.x,p2.y\n",
    "return [(float(xs1),float(ys1)),(float(xs2),float(ys2))]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "deg_to_rad(225)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 77,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1.8778968735922943\n"
     ]
    }
   ],
   "source": [
    "radius = 3.0\n",
    "beta = 0.62596562453076476\n",
    "\n",
    "print(arclength(radius, beta))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 149,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "#2\n"
     ]
    },
    {
     "ename": "ValueError",
     "evalue": "math domain error",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mValueError\u001b[0m                                Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-149-83d3a34817d9>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m     37\u001b[0m \u001b[0mx2\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0my2\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mradius2\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmid_circle\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mx\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0my\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mGoal\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     38\u001b[0m \u001b[0mcircle2\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mx2\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0my2\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mradius2\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 39\u001b[0;31m \u001b[0mt\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0mintersection\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mcircle1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcircle2\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     40\u001b[0m \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     41\u001b[0m \u001b[0;31m#t[0][0]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m<ipython-input-149-83d3a34817d9>\u001b[0m in \u001b[0;36mintersection\u001b[0;34m(circle1, circle2)\u001b[0m\n\u001b[1;32m     18\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     19\u001b[0m     \u001b[0ma\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0mr1\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0mr1\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0mr2\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0mr2\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0md\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0md\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m/\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m2\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0md\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 20\u001b[0;31m     \u001b[0mh\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0msqrt\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mr1\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0mr1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0ma\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0ma\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     21\u001b[0m     \u001b[0mxm\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mx1\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0ma\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0mdx\u001b[0m\u001b[0;34m/\u001b[0m\u001b[0md\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     22\u001b[0m     \u001b[0mym\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0my1\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0ma\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0mdy\u001b[0m\u001b[0;34m/\u001b[0m\u001b[0md\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mValueError\u001b[0m: math domain error"
     ]
    }
   ],
   "source": [
    "def intersection(circle1, circle2):\n",
    "    x1,y1 = circle1[0][0], circle1[0][1]\n",
    "    r1  = circle1[1] \n",
    "    x2,y2 = circle2[0][0], circle2[0][1]\n",
    "    r2 = circle2[1]\n",
    "        # http://stackoverflow.com/a/3349134/798588\n",
    "    dx,dy = x2-x1,y2-y1\n",
    "    d = sqrt(dx*dx+dy*dy)\n",
    "    if d > r1+r2:\n",
    "        print (\"#1\")\n",
    "        return None # no solutions, the circles are separate\n",
    "    if d < abs(r1-r2):\n",
    "        print (\"#2\")\n",
    "        #return None # no solutions because one circle is contained within the other\n",
    "    if d == 0 and r1 == r2:\n",
    "        print (\"#3\")\n",
    "        return None # circles are coincident and there are an infinite number of solutions\n",
    "\n",
    "    a = (r1*r1-r2*r2+d*d)/(2*d)\n",
    "    h = sqrt((r1*r1)-(a*a))\n",
    "    xm = x1 + a*dx/d\n",
    "    ym = y1 + a*dy/d\n",
    "    xs1 = xm + h*dy/d\n",
    "    xs2 = xm - h*dy/d\n",
    "    ys1 = ym - h*dx/d\n",
    "    ys2 = ym + h*dx/d\n",
    "\n",
    "    return [float(xs1),float(ys1),(float(xs2),float(ys2))]\n",
    "\n",
    "\n",
    "circle1 = [[3,0],3]\n",
    "\n",
    "x = 0\n",
    "y = 0\n",
    "Goal = goal(100,100)\n",
    "\n",
    "x2,y2,radius2 = mid_circle(x,y,Goal)     \n",
    "circle2 = [[x2,y2],radius2]\n",
    "t = (intersection(circle1, circle2))\n",
    "print(t)\n",
    "#t[0][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 237,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[2.5, 2.5], 3.5355339059327378]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "0.0"
      ]
     },
     "execution_count": 237,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "circle1 = [[3,0],3]\n",
    "\n",
    "x = 0\n",
    "y = 0\n",
    "Goal = goal(5,5)\n",
    "\n",
    "x2,y2,radius2 = mid_circle(x,y,Goal)\n",
    "circle2 = [[x2,y2],radius2]\n",
    "print(circle2)\n",
    "# def tang_from_point(circle1, circle2): # circle = [[α, β], r] ; point [xP, yP]\n",
    "\n",
    "#     x1,y1 = circle1[0][0], circle1[0][1]\n",
    "#     r1  = circle1[1] \n",
    "#     x2,y2 = circle2[0][0], circle2[0][1]\n",
    "#     r2 = circle2[1]\n",
    "#     c1=Circle(Point(x1,y1),r1)\n",
    "#     c2=Circle(Point(x2,y2),r2)\n",
    "#     intersection = c1.intersection(c2)\n",
    "#     if len(intersection) == 1:\n",
    "#         intersection.append(intersection[0])\n",
    "#     p1 = intersection[0]\n",
    "#     p2 = intersection[1]\n",
    "#     xs1,ys1 = p1.x,p1.y\n",
    "#     xs2,ys2 = p2.x,p2.y\n",
    "#     return ((float(xs1),float(ys1)),(float(xs2),float(ys2)))\n",
    "t = (tang_from_point(circle1, circle2))\n",
    "t[0][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 63,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-1.1666666666666667"
      ]
     },
     "execution_count": 63,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "-7/6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 62,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-2.763853991962833"
      ]
     },
     "execution_count": 62,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "-5*sqrt(11)/6\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def tangent(circle,point):\n",
    "    cx = circle[0][0]\n",
    "    cy = circle[0][1]\n",
    "    radius = circle[1]\n",
    "    px = point[0]\n",
    "    py = point[1]\n",
    "    distance = math.sqrt(( float(cx-px) **2 + float(cy-py)**2))\n",
    "    #d=sqr((x1-x0)^2 + (y1-y0)^2)\n",
    "    a=(r0^2-r1^2+d^2)/(2*d)\n",
    "    h=sqr(r0^2-a^2)\n",
    "    x2=x0+a*(x1-x0)/d   \n",
    "    y2=y0+a*(y1-y0)/d   \n",
    "    x3=x2+h*(y1-y0)/d       #// also x3=x2-h*(y1-y0)/d\n",
    "    y3=y2-h*(x1-x0)/d       #// also y3=y2+h*(x1-x0)/d\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 95,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(0.90165559515004534, 1.0, 0.62596562453076476)\n"
     ]
    }
   ],
   "source": [
    "A = [0,0]\n",
    "B = [-1,0]\n",
    "C = [-1,2] \n",
    "    \n",
    "print(get_angle(A,B,C))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0.0, 0.0], 3.0]\n",
      "[[-0.1364, 0.6818], [0.1364, -0.6818]]\n",
      "5.0\n"
     ]
    }
   ],
   "source": [
    "alpha_c,beta_c = 0.0, 0.0\n",
    "Goal = goal(5.0,0.0)\n",
    "radius = 3.0\n",
    "target = [Goal.x, Goal.y]\n",
    "circle = [[alpha_c,beta_c],radius]\n",
    "print(circle)\n",
    "print(tang_from_point(circle,[Goal.x , Goal.y]))\n",
    "print(np.sqrt( ((target[0]-alpha_c)**2) + ((target[1]-beta_c)**2) ))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 257,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def search():\n",
    "\n",
    "#     topic = 'visualization_marker_array'\n",
    "#     publisher = rospy.Publisher(topic, MarkerArray, queue_size=100)\n",
    "\n",
    "#     markerArray = MarkerArray()\n",
    "\n",
    "\n",
    "    goal_reached = 0\n",
    "    grid_goal = 0\n",
    "    counter = 0\n",
    "    time_out = 45000\n",
    "    low_h = 0\n",
    "    low_index = 0\n",
    "    for count in range(2):\n",
    "        #start = time.time()\n",
    "        while ( len(open_list) > 0 ) & (goal_reached == 0) & (counter<time_out) & (grid_goal ==0 ):\n",
    "            \n",
    "            counter+=1\n",
    "\n",
    "            open_list.sort( key=lambda ste:float(ste.state.f) )\n",
    "\n",
    "            #print(\"Pop Value\",open_list[0].state.steer)\n",
    "            path_link = open_list.pop(0)\n",
    "            #print(\"The Steering angle:\",path_link.state.steer)\n",
    "            path_state = path_link.state\n",
    "           # print(path_link.state.steer)\n",
    "            path_index = path_link.index\n",
    "\n",
    "           # print( \"\" )\n",
    "            #print( \"Best path\")\n",
    "            #print( path_state.x, path_state.y, path_state.theta, path_state.f )\n",
    "            path_x.append( path_state.x )\n",
    "            path_y.append( path_state.y )\n",
    "           \n",
    "\n",
    "            #exit = 0\n",
    "\n",
    "            #while exit == 0:\n",
    "\n",
    "            delta_angle = delta_t( path_state.steer, 4, 0.0872665 )\n",
    "\n",
    "            #delta_angle = [ 30 * np.pi / 180, 60 * np.pi / 180, 0, -60 * np.pi / 180, -30 * np.pi / 180 ]\n",
    "\n",
    "            #print( delta_angle )\n",
    "\n",
    "            state_list = []\n",
    "\n",
    "            for i in range(len(delta_angle)):\n",
    "                \n",
    "                new_state = transition( path_state, delta_angle[i], 0.1, Goal)\n",
    "                \n",
    "                #print( new_state.x, new_state.y, new_state.theta )\n",
    "                \n",
    "                if checkLimits( new_state.x , new_state.y ) == True:\n",
    "                    if check_closed( new_state.x, new_state.y, new_state.theta ) == 0:                \n",
    "                        state_list.append(new_state)\n",
    "                        set_closed( new_state.x, new_state.y, new_state.theta )\n",
    "\n",
    "                else:\n",
    "\n",
    "\n",
    "                        X = Goal.x-new_state.x \n",
    "                        Y = Goal.y-new_state.y \n",
    "                        angleInRadian = np.arctan2(Y,X)\n",
    "\n",
    "                        diff_theta = np.abs(angleInRadian - new_state.theta)\n",
    "                        #print(diff_theta)\n",
    "                        if diff_theta< 0.523599:\n",
    "                            \n",
    "                            grid_goal = 1\n",
    "                            break\n",
    "    \n",
    "                        \n",
    "            if grid_goal == 1 : \n",
    "                break\n",
    "                    \n",
    "    \n",
    "                        \n",
    "                        \n",
    "\n",
    "            collision_free = []\n",
    "\n",
    "            for i in range(len(state_list)):\n",
    "\n",
    "                check = 0\n",
    "                new_points = transformation(vehicle_points, state_list[i].x, state_list[i].y, state_list[i].theta)\n",
    "                #new_points = [ [ state_list[i].x, state_list[i].y ] ]\n",
    "                \n",
    "                for point in new_points :\n",
    "                    if checkLimits(point[0],point[1]) == True:\n",
    "\n",
    "                        #get_x, get_y = get_grid_coordinates(point[0], point[1])\n",
    "                        \n",
    "                        check = check_occupancy(point[0], point[1])\n",
    "                       \n",
    "                    else :\n",
    "\n",
    "                        X = Goal.x-state_list[i].x \n",
    "                        Y = Goal.y-state_list[i].y \n",
    "                        angleInRadian = np.arctan2(Y,X)\n",
    "\n",
    "                        diff_theta = np.abs(angleInRadian - state_list[i].theta)\n",
    "                        print(diff_theta)\n",
    "                        if diff_theta< 0.523599:\n",
    "                            \n",
    "                            grid_goal = 1\n",
    "                            break\n",
    "                        \n",
    "                        check =1\n",
    "\n",
    "                    if check == 1:\n",
    "                    \n",
    "                       break\n",
    "\n",
    "                if check == 0:\n",
    "                    collision_free.append(state_list[i])\n",
    "                    \n",
    "\n",
    "            if( collision_free ):\n",
    "\n",
    "                for nstate in collision_free:\n",
    "                    \n",
    "                    #print( nstate.x, nstate.y, nstate.theta )\n",
    "                    new_link = link( nstate, len(trace_list) )\n",
    "                    open_list.append( new_link )\n",
    "                    \n",
    "                    \n",
    "                    \n",
    "                    trace_link = link( nstate, path_index )\n",
    "\n",
    "                    trace_list.append( trace_link )\n",
    "                    \n",
    "                    if (low_h == 0) :\n",
    "                        low_h =  nstate.h \n",
    "                        low_index = len(trace_list) - 1\n",
    "                        \n",
    "                    elif low_h > nstate.h:\n",
    "                        low_h =  nstate.h \n",
    "                        low_index = len(trace_list) - 1\n",
    "                                \n",
    "                                \n",
    "                                \n",
    "                              \n",
    "                    \n",
    "                    if np.sqrt((nstate.x - Goal.x)**2 + (nstate.y - Goal.y)**2) < 0.5:\n",
    "                        goal_reached = 1\n",
    "                        path_x.append( nstate.x )\n",
    "                        path_y.append( nstate.y )\n",
    "                        print( \"goal\")\n",
    "                        #end = time.time()\n",
    "                        #print(\"Process Time\", end - start)\n",
    "            #print(len(trace_list))\n",
    "\n",
    "            \n",
    "\n",
    "\n",
    "    if goal_reached == 1 :\n",
    "            fpath_x = []\n",
    "            fpath_y = []\n",
    "            fpath_steer = []\n",
    "            \n",
    "            curr_trace = trace_list[-1]\n",
    "            \n",
    "            while( curr_trace.index != -1 ):\n",
    "                fpath_x.append( curr_trace.state.x )\n",
    "                fpath_y.append( curr_trace.state.y )\n",
    "                fpath_steer.append( curr_trace.state.steer )\n",
    "                curr_trace = trace_list[ curr_trace.index ]\n",
    "                \n",
    "            plt.scatter( fpath_x, fpath_y)\n",
    "\n",
    "\n",
    "            #print(fpath_x[-2], fpath_y[-2])\n",
    "\n",
    "            #end = time.time()\n",
    "            #print(\"Process Time\", end - start)\n",
    "\n",
    "            plt.axis( 'equal' )\n",
    "            #plt.show()\n",
    "            plt.savefig('path.png')\n",
    "            print(fpath_steer[-2])\n",
    "            #return fpath_steer[-2]\n",
    "\n",
    "\n",
    "\n",
    "    if (goal_reached == 0) & (grid_goal ==0 ) :\n",
    "            fpath_x = []\n",
    "            fpath_y = []\n",
    "            fpath_steer = []\n",
    "            \n",
    "            print(\"low_index:\",low_index)\n",
    "            curr_trace = trace_list[low_index]\n",
    "            \n",
    "            while( curr_trace.index != -1 ):\n",
    "                fpath_x.append( curr_trace.state.x )\n",
    "                fpath_y.append( curr_trace.state.y )\n",
    "                fpath_steer.append( curr_trace.state.steer )\n",
    "                curr_trace = trace_list[ curr_trace.index ]\n",
    "                    \n",
    "            plt.scatter( fpath_x, fpath_y)\n",
    "\n",
    "            #print(fpath_x[-2], fpath_y[-2])\n",
    "            \n",
    "\n",
    "            plt.axis( 'equal' )\n",
    "            #plt.show()\n",
    "            plt.savefig('path.png')\n",
    "            print(fpath_steer[-2])\n",
    "            #return fpath_steer[-2]\n",
    "\n",
    "\n",
    "    if grid_goal == 1:\n",
    "            #print(\"here\")\n",
    "            fpath_x = []\n",
    "            fpath_y = []\n",
    "            fpath_steer = []\n",
    "            \n",
    "            curr_trace = trace_list[path_index]\n",
    "            \n",
    "            while( curr_trace.index != -1 ):\n",
    "                fpath_x.append( curr_trace.state.x )\n",
    "                fpath_y.append( curr_trace.state.y )\n",
    "                fpath_steer.append( curr_trace.state.steer )\n",
    "                next_trace = curr_trace\n",
    "                curr_trace = trace_list[ curr_trace.index ]\n",
    "              \n",
    "            plt.scatter( fpath_x, fpath_y )\n",
    "\n",
    "            \n",
    "            print(fpath_steer[-2])\n",
    "            #end = time.time()\n",
    "            #print(\"Process Time\", end - start)\n",
    "\n",
    "\n",
    "\n",
    "            plt.axis( 'equal' )\n",
    "            #plt.show()\n",
    "            plt.savefig('path.png')\n",
    "            #return fpath_steer[-2]\n",
    "     \n",
    "#     for i in range( len(fpath_x) ):\n",
    "            \n",
    "#             marker = Marker()\n",
    "#             marker.header.frame_id = \"/neck\"\n",
    "#             marker.id = i\n",
    "#             marker.type = marker.SPHERE\n",
    "#             marker.action = marker.ADD\n",
    "#             marker.scale.x = 0.2\n",
    "#             marker.scale.y = 0.2\n",
    "#             marker.scale.z = 0.2\n",
    "#             marker.color.a = 1.0\n",
    "#             marker.color.r = 1.0\n",
    "#             marker.color.g = 1.0\n",
    "#             marker.color.b = 0.0\n",
    "#             #marker.lifetime = rospy.Duration(0.5)\n",
    "#             marker.pose.orientation.w = 1.0\n",
    "#             marker.pose.position.x = fpath_x[i] + x_offset\n",
    "#             marker.pose.position.y = fpath_y[i] + y_offset\n",
    "#             marker.pose.position.z = 2\n",
    "\n",
    "#             markerArray.markers.append(marker)\n",
    "\n",
    "#     publisher.publish(markerArray)\n",
    "    return fpath_steer[-2]\n",
    "\n",
    "\n",
    "# def steerCallback(msg):\n",
    "#     current_state.steer = msg.data\n",
    "    \n",
    "# def callback(msg):\n",
    "\n",
    "#     global grid\n",
    "\n",
    "#     for i in range(grid.shape[0]):\n",
    "#             for j in range(grid.shape[1]):\n",
    "#                 grid[j,i] = msg.data[ grid.shape[0] * i + j ]\n",
    "\n",
    "#     #print(grid_data)\n",
    "\n",
    "#     f = open( \"grid.csv\", \"w\" )\n",
    "  \n",
    "#     for i in range(grid.shape[0]):\n",
    "#             for j in range(grid.shape[1]):\n",
    "#                 f.write( np.str( grid[i][j] ) )\n",
    "#                 f.write( \", \" )\n",
    "#             f.write( \"\\n\" )\n",
    "\n",
    "#     f.close()\n",
    "               \n",
    "#     coll_cell_side =  msg.info.resolution\n",
    "\n",
    "    \n",
    "\n",
    "#     search_service()   \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 258,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       "       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])"
      ]
     },
     "execution_count": 258,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "grid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 261,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5707963267948966\n",
      "1.5715380230432514\n",
      "1.5715380230432514\n"
     ]
    },
    {
     "ename": "TypeError",
     "evalue": "unsupported operand type(s) for +: 'float' and 'NoneType'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-261-74fd2bf41715>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[0;32mif\u001b[0m \u001b[0m__name__\u001b[0m\u001b[0;34m==\u001b[0m \u001b[0;34m\"__main__\"\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      2\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 3\u001b[0;31m     \u001b[0msearch\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[0;32m<ipython-input-257-adc431bae909>\u001b[0m in \u001b[0;36msearch\u001b[0;34m()\u001b[0m\n\u001b[1;32m     49\u001b[0m             \u001b[0;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mlen\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdelta_angle\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     50\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 51\u001b[0;31m                 \u001b[0mnew_state\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mtransition\u001b[0m\u001b[0;34m(\u001b[0m \u001b[0mpath_state\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdelta_angle\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0.1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mGoal\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     52\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     53\u001b[0m                 \u001b[0;31m#print( new_state.x, new_state.y, new_state.theta )\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m<ipython-input-260-fd85396a2877>\u001b[0m in \u001b[0;36mtransition\u001b[0;34m(v_state, delta, dt, goal)\u001b[0m\n\u001b[1;32m     64\u001b[0m     \u001b[0mnext_h\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mheuristic_2\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mnext_x\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mnext_y\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mgoal\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     65\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 66\u001b[0;31m     \u001b[0mnext_f\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnext_g\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0mnext_h\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0;36m0.1\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdelta\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     67\u001b[0m     \u001b[0mnext_steer\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdelta\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     68\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mTypeError\u001b[0m: unsupported operand type(s) for +: 'float' and 'NoneType'"
     ]
    }
   ],
   "source": [
    "if __name__== \"__main__\":\n",
    "    \n",
    "    search()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[3, 4, 4, 5, 5, 6, 7, 7, 8, 33, 66, 666]"
      ]
     },
     "execution_count": 39,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a = [5,3,4,5,6,7,8,33,4,666,7,66]\n",
    "sorted(a)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[5, 3, 4, 5, 6, 7, 8, 33, 4, 666, 7, 66]"
      ]
     },
     "execution_count": 40,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.3"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
